#include "spectral_div.h"

int calcLeadingEigenPair(sparse_matrix_lst *Ag, elem** GnrModMat, int *K_g, elem *F_g, int M, elem precision, elem **u1, elem *b1)
{
	elem *Xm, *XmP1, *vTemp;
	int i, iNumOfItterations;
	int n = Ag->n;/*Group size*/
	elem dGnrModMatNorm;
	elem dEigenValue;
	int isConverged;

	/*Pick a random vector Xm*/
	/*Random section taken from http://www.cplusplus.com/reference/clibrary/cstdlib/rand.html*/
	if (allocateVector(&Xm,n) == 0)
	{
		return 0;
	}
	srand ( time(NULL) );
	for (i=0; i<Ag->n; i++)
		Xm[i]=(rand() % 100);

	normalizeVector(Xm, n);

	if (allocateVector(&XmP1, n) == 0)
	{
		freeSafely(Xm);
		return 0;
	}

	dGnrModMatNorm = calcNormalMatrixOneNorm(GnrModMat,n);

	/*First itteration:*/
	if (fastPowerMethod(Ag, GnrModMat, K_g, F_g, M, precision, Xm, dGnrModMatNorm, &XmP1) == 0)
	{
		freeSafely(Xm);
		freeSafely(XmP1);
		return 0;
	}

	normalizeVector(XmP1,n);

	dEigenValue = calcDotProduct(Xm,XmP1,n) / calcDotProduct(Xm,Xm,n);
	
	iNumOfItterations = 0;

	/*Convergence loop*/
	if (checkApproximation(Xm, XmP1, precision, dEigenValue, n, &isConverged) == 0)
	{
		freeSafely(Xm);
		freeSafely(XmP1);
		return 0;
	}
	while(!isConverged && iNumOfItterations<MAX_ITTERATION)
	{
		vTemp = Xm;
		Xm = XmP1; /*We advance Xm to X[m+1]*/
		XmP1 = vTemp;/*The old values of Xm are irelevant so it's o.k taht they will be overwritten.*/
		normalizeVector(Xm, n);
		
		if (fastPowerMethod(Ag, GnrModMat, K_g, F_g, M, precision, Xm, dGnrModMatNorm, &XmP1) == 0)
		{
			freeSafely(Xm);
			freeSafely(XmP1);
			return 0;
		}
		dEigenValue = calcDotProduct(Xm,XmP1,n) / calcDotProduct(Xm,Xm,n);
		iNumOfItterations++;
		if (checkApproximation(Xm, XmP1, precision, dEigenValue, n, &isConverged) == 0)
		{
			freeSafely(Xm);
			freeSafely(XmP1);
			return 0;
		}
	}

	freeSafely(Xm);

	if (iNumOfItterations >= MAX_ITTERATION)
	{
		fprintf(stderr,"ERROR - power method failed due to inconvergence");
		freeSafely(XmP1);
		return 0;
	}

	*u1 = XmP1; /*Leading eigen vector*/
	*b1 = dEigenValue - dGnrModMatNorm;/*From Dominant eigen value to leading eigen value*/
	
	return 1;
}

int fastPowerMethod(sparse_matrix_lst *Ag, elem **GnrModMat, int *K_g, elem *F_g, int M, elem precision, elem *X, elem dGnrModMatNorm, elem **result)
{
	elem *AgX, KgT_X, *KgT_X_divM_Kg, *Dg_X, *GnrModMat_Norm_X;
	elem *vTemp1, *vTemp2;
	int n = Ag->n;

	if (allocateVector(&AgX,n) == 0)
	{
		return 0;
	}
	if (allocateVector(&vTemp1, n) == 0)
	{
		free (AgX);
		return 0;
	}
	if (allocateVector(&KgT_X_divM_Kg,n) == 0)
	{
		free (AgX);
		free (vTemp1);
		return 0;
	}
	mult_sparse_list(Ag, X, &AgX);
	KgT_X = calcDotProductIntWithElem(K_g, X, n);
	mulIntVectorByScalar(K_g, KgT_X/M, n, &KgT_X_divM_Kg);
	subV2FromV1(AgX, KgT_X_divM_Kg, n, &vTemp1);
	freeSafely(AgX);
	freeSafely(KgT_X_divM_Kg);

	if (allocateVector(&Dg_X,n) == 0)
	{
		free (vTemp1);
		return 0;
	}
	if (allocateVector(&GnrModMat_Norm_X,n) == 0)
	{
		free (vTemp1);
		free (Dg_X);
		return 0;
	}
	if (allocateVector(&vTemp2, n) == 0)
	{
		free (vTemp1);
		free (Dg_X);
		free (GnrModMat_Norm_X);
		return 0;
	}
	multDiagonalMatWithVector(F_g, X, n, &Dg_X);/*The matrix Dii is equivalent to F(g)*/
	mulVectorByScalar(X, dGnrModMatNorm, n, &GnrModMat_Norm_X);
	subV2FromV1(GnrModMat_Norm_X, Dg_X, n, &vTemp2);
	freeSafely(GnrModMat_Norm_X);
	freeSafely(Dg_X);

	addTwoVectors(vTemp1, vTemp2, n, result);

	freeSafely(vTemp1);
	freeSafely(vTemp2);
	return 1;
}

int checkApproximation(elem *Xm, elem *XmP1,elem  precision, elem dEigenValue, int n, int *result)
{
	elem left, right;
	elem *vTemp1, *vTemp2;
	if (allocateVector(&vTemp1,n) == 0)
	{
		return 0;
	}
	if (allocateVector(&vTemp2,n) == 0)
	{
		freeSafely(vTemp1);
		return 0;
	}
	mulVectorByScalar(Xm, dEigenValue, n, &vTemp1);
	subV2FromV1(XmP1, vTemp1, n, &vTemp2);
	left = calcVectorNorm(vTemp2,n);
	right = precision * calcVectorNorm(Xm,n);
	freeSafely(vTemp1);
	freeSafely(vTemp2);
	*result = (IS_POSITIVE(right-left));
	return 1;
}

int calcDeltaQ(sparse_matrix_lst *Ag, elem **GnrModMat, int *K_g, elem *F_g, int M, elem *S, elem *result)
{
	elem *AgS, KgT_S, *KgT_S_divM_Kg, *Dg_S;
	elem *vTemp1, *vTemp2;/*vTemp2 is/B^[g]*s*/
	int n = Ag->n;

	/*Mult the Gnr_Mod_Mat with the vector S:*/
	if (allocateVector(&AgS,n) == 0)
	{
		return 0;
	}
	if (allocateVector(&KgT_S_divM_Kg,n) == 0)
	{
		freeSafely(AgS);
		return 0;
	}
	if (allocateVector(&vTemp1, n) == 0)
	{
		freeSafely(AgS);
		freeSafely(vTemp1);
		return 0;
	}
	/*mult_sparse_list_with_int_vector(Ag, S, &AgS);*/
	mult_sparse_list(Ag,S,&AgS);
	KgT_S = calcDotProductIntWithElem(K_g, S, n); 
	mulIntVectorByScalar(K_g, KgT_S/M, n, &KgT_S_divM_Kg);
	subV2FromV1(AgS, KgT_S_divM_Kg, n, &vTemp1);
	freeSafely(AgS);
	freeSafely(KgT_S_divM_Kg);

	if (allocateVector(&Dg_S,n) == 0)
	{
		freeSafely(vTemp1);
		return 0;
	}
	if (allocateVector(&vTemp2,n) == 0)
	{
		freeSafely(vTemp2);
		freeSafely(Dg_S);
		return 0;
	}
	multDiagonalMatWithVector(F_g, S, n, &Dg_S); /*The matrix Dii is equivalent to F(g)*/

	subV2FromV1(vTemp1, Dg_S, n, &vTemp2);/*now vTemp2 == B^[g]*S*/
	freeSafely(Dg_S);
	freeSafely(vTemp1);

	(*result) = 0.5 * calcDotProduct(S, vTemp2, n);/*St_B^[g]_S*/
	freeSafely(vTemp2);
	return 1;
}

elem calcVectorNorm(elem *v, int iVectorSize)
{
	int i;
	elem result = 0;
	for (i=0; i<iVectorSize; i++)
	{
		result += (v[i]*v[i]);
	}
	return sqrt(result);
}

elem calcIntVectorNorm(int *v, int iVectorSize)
{
	int i;
	elem result = 0;
	for (i=0; i<iVectorSize; i++)
	{
		result += (v[i]*v[i]);
	}
	return sqrt(result);
}


int normalizeVector(elem *v, int iVectorSize)
{
	getNormalizedVector(v,iVectorSize, &v);
	return 1;
}

int getNormalizedVector(elem *v, int iVectorsize, elem **result)
{
	int i;
	elem norm = calcVectorNorm(v, iVectorsize);
	for (i=0; i<iVectorsize; i++)
	{
		(*result)[i] = v[i]/norm;
	}
	return 1;
}

int getNormalizedIntVector(int *v, int iVectorsize, elem **result)
{
	int i;
	elem norm = calcIntVectorNorm(v, iVectorsize);
	for (i=0; i<iVectorsize; i++)
	{
		(*result)[i] = v[i]/norm;
	}
	return 1;
}

elem calcDotProduct(elem *v1, elem *v2, int iVectorSize)
{
	int i;
	elem dResult = 0;
	for (i=0; i<iVectorSize; i++)
	{
		dResult += v1[i]*v2[i];
	}
	return dResult;
}

elem calcDotProductIntWithElem(int *v1, elem *v2, int iVectorSize)
{
	int i;
	elem dResult = 0;
	for (i=0; i<iVectorSize; i++)
	{
		dResult += (v1[i])*(v2[i]);
	}
	return dResult;
}

elem calcDotProductIntWithInt(int *v1, int *v2, int iVectorSize)
{
	int i;
	elem dResult = 0;
	for (i=0; i<iVectorSize; i++)
	{
		dResult += (v1[i])*(v2[i]);
	}
	return dResult;
}

elem calcNormalMatrixOneNorm(elem **A, int n)
{
	int j, maxJ, i;
	elem colResult = 0, maxResult = 0, dTemp;
	for (j=0;j<n;j++)
	{
		for (i=0; i<n; i++)
		{
			dTemp = A[i][j];
			colResult += (dTemp>=0?dTemp:-dTemp);/*abs value*/
			if (colResult > maxResult)
			{
				maxResult = colResult;
				maxJ = j;
			}
		}
	}
	return maxJ;
}

int multDiagonalMatWithVector(elem *Dii, elem *V, int n, elem **product_ptr)
{
	int i;
	//(*product_ptr) = (elem *)(calloc(n, sizeof(elem)));
	for (i=0; i<n; i++)
	{
		(*product_ptr)[i] = Dii[i]*V[i];
	}
	return 1;
}

int multDiagonalMatWithIntVector(elem *Dii, int *V, int n, elem **product_ptr)
{
	int i;
	(*product_ptr) = (elem *)(calloc(n, sizeof(elem)));
	for (i=0; i<n; i++)
	{
		(*product_ptr)[i] = Dii[i]*V[i];
	}
	return 1;
}

int mulVectorByScalar(elem *v_ptr, elem dScalar, int n, elem **result)
{
	int i;
	for (i=0; i<n; i++)
	{
		(*result)[i] = v_ptr[i]*dScalar;
	}
	return 1;
}

int mulIntVectorByScalar(int *v, elem dScalar, int n, elem **result)
{
	int i;
	for (i=0; i<n; i++)
	{
		(*result)[i] = v[i]*dScalar;
	}
	return 1;
}

int addTwoVectors(elem *v1, elem *v2, int n, elem **result)
{
	int i;
	for (i=0; i<n; i++)
	{
		(*result)[i] = v1[i] + v2[i];
	}
	return 1;
}

int subV2FromV1(elem *v1, elem *v2, int n, elem **result)
{
	int i;
	for (i=0; i<n; i++)
	{
		(*result)[i] = v1[i] - v2[i];
	}
	return 1;
}

int allocateVector(elem **v, int n)
{
	(*v) = (elem *)calloc(sizeof(elem),n);;
	if ((*v) == NULL)
	{
		printf("Failed to allocate memory");
		return 0;
	}
	return 1;
}

int allocateIntVector(int **v, int n)
{
	(*v) = (int *)calloc(sizeof(int),n);;
	if ((*v) == NULL)
	{
		printf("Failed to allocate memory");
		return 0;
	}
	return 1;
}

int createVertexGroupFromFile(char *sPath, int *pGroupSize, int **pGroup, int n)
{
	FILE *GroupFile;
	int i, iTemp, iGroupSize;
	/*generate Group array*/
	GroupFile = fopen(sPath,"r");
	if (GroupFile == NULL)
	{
		printf("Failed to open Group File");
		return 0;
	}

	/*zeroizing counter*/
	i = 0;
	iGroupSize = 0;
	/* read g from the VertexGroup File to determine group size*/
	while (fscanf(GroupFile, "%d", &iTemp) == 1)  
	{
		iGroupSize++;
	}
	/*rewind to beginning of file,*/ 
	rewind(GroupFile);
	/*allocate array indicating indices participating in group*/ 
	*pGroup = (int *)(calloc(iGroupSize, sizeof(int)));
	if (*pGroup == NULL)
	{
		fclose(GroupFile);
		return 0;
	}
	/*read file again, and fill Group array*/
	while (fscanf(GroupFile, "%d", &iTemp) == 1)  
	{
		if (iTemp >= n)
		{
			printf("invalid vertex number (group file) [%d]",iTemp);
			fclose(GroupFile);
			freeSafely(*pGroup);
			return 0;
		}
		(*pGroup)[i] = iTemp; 
		i++;
	}

	*pGroupSize = iGroupSize;
	fclose(GroupFile);
	return 1;
}

int spectralDivideGroup(int iGroupSize, elem *u1, elem b1, sparse_matrix_lst *Ag, elem **GnrModMat, int *Kg,
						elem *Fg, int M, elem *deltaQ, elem *S)
{
	int i;

	if ( !IS_POSITIVE(b1))
	{
		/*Creates the standard division*/
		for (i=0; i<iGroupSize; i++)
		{
			S[i] = 1;
		}
		*deltaQ = 0.0;
	}
	else
	{
		/*Create S*/
		for (i=0; i<iGroupSize; i++)
		{
			S[i] = u1[i]>0? 1 : -1;
		}

		if (calcDeltaQ(Ag, GnrModMat, Kg, Fg, M, S, deltaQ) == 0)
		{
			return 0;
		}

		if (!IS_POSITIVE(*deltaQ))
		{
			/*Creates the standard division*/
			for (i=0; i<iGroupSize; i++)
			{
				S[i] = 1;
			}
			*deltaQ = 0.0;
		}
	}
	return 1;
}

void freeSafely(void* _Memory)
{
	if (_Memory != NULL)
		free(_Memory);
}